/*
 * Copyright 2018 The Cartographer Authors
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 *      http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 */

#include "scan_matching/occupied_grid_cost_function_2d.h"

namespace cartographer {
namespace scan_matching {

bool OccupiedGridCostFunction2D::ComputeResidualAndJacobianPoseGraph(const State& state, const Eigen::Matrix<double, 3, 1>& point, Eigen::Matrix<double, 1, 1> &residualXi, Eigen::Matrix<double, 1, 3> &jacobianXi) {
    Eigen::Matrix<double, 2, 1> translation = state.p;
    Eigen::Rotation2D rotation(SO2(state.R).log());
    Eigen::Matrix<double, 2, 2> rotation_matrix = rotation.toRotationMatrix();
    Eigen::Matrix<double, 3, 3> transform;
    transform << rotation_matrix, translation, 0., 0., 1.;
        
    // LOG(INFO) << " transform ===== : " << transform << std::endl;

    const MapLimits& limits = grid_.limits();

    // 根据预测位姿对单个点进行坐标变换
    const Eigen::Matrix<double, 3, 1> world = transform * point;
    Eigen::Vector2d world_pose = world.head<2>();
    Eigen::Array2d cell_double_idx = limits.GetCellDoubleIndex(world_pose); 

    // Bilinear Interpolation
    Eigen::Array2i cell_idx00 = limits.GetCellIndex(Eigen::Vector2f(world[0], world[1])); 
    Eigen::Array2i cell_idx10(cell_idx00[0] + 1, cell_idx00[1]);
    Eigen::Array2i cell_idx01(cell_idx00[0], cell_idx00[1] + 1);
    Eigen::Array2i cell_idx11(cell_idx00[0] + 1, cell_idx00[1] + 1);

    // LOG(INFO) << " cell_double_idx ===== : " << cell_double_idx.transpose() << std::endl;
    // LOG(INFO) << " cell_idx00 ===== : " << cell_idx00.transpose() << std::endl;
    // LOG(INFO) << " cell_idx10 ===== : " << cell_idx10.transpose() << std::endl;
    // LOG(INFO) << " cell_idx01 ===== : " << cell_idx01.transpose() << std::endl;
    // LOG(INFO) << " cell_idx11 ===== : " << cell_idx11.transpose() << std::endl;

      if(!limits.Contains(cell_idx00) || !limits.Contains(cell_idx10) ||
         !limits.Contains(cell_idx01) || !limits.Contains(cell_idx11)) {
          // LOG(INFO) << " jacobians[0] ===== : " << jacobians[0][0] << " , " << jacobians[0][1] << " , " << jacobians[0][2] << std::endl;
          // LOG(INFO) << " scaling_factor ===== : " << scaling_factor << std::endl;
          return false;
      }

      // double p00 = static_cast<const ProbabilityGrid&>(grid_).GetProbability(cell_idx00);
      // double p10 = static_cast<const ProbabilityGrid&>(grid_).GetProbability(cell_idx10);
      // double p01 = static_cast<const ProbabilityGrid&>(grid_).GetProbability(cell_idx01);
      // double p11 = static_cast<const ProbabilityGrid&>(grid_).GetProbability(cell_idx11);

      // LOG(INFO) << " cell_double_idx ===== : " << cell_double_idx.transpose() << std::endl;

      float p00 = nearest_search_.ComputeProbBilinearInterpolate(cell_idx00);
      float p10 = nearest_search_.ComputeProbBilinearInterpolate(cell_idx10);
      float p01 = nearest_search_.ComputeProbBilinearInterpolate(cell_idx01);
      float p11 = nearest_search_.ComputeProbBilinearInterpolate(cell_idx11);

      // LOG(INFO) << " p00 ===== : " << p00 << std::endl;
      // LOG(INFO) << " p10 ===== : " << p10 << std::endl;
      // LOG(INFO) << " p01 ===== : " << p01 << std::endl;
      // LOG(INFO) << " p11 ===== : " << p11 << std::endl;

      double x = cell_double_idx[0];
      double y = cell_double_idx[1];

      double x0 = cell_idx00[0];
      double y0 = cell_idx00[1];
      double x1 = cell_idx11[0];
      double y1 = cell_idx11[1];    

      // LOG(INFO) << " y1-y0 ===== : " << y1-y0 << std::endl;
      // LOG(INFO) << " x1-x0 ===== : " << x1-x0<< std::endl;

      double probability = (y-y0)/(y1-y0) * ((x-x0)/(x1-x0)*p11+(x1-x)/(x1-x0)*p01) +
                           (y1-y)/(y1-y0) * ((x-x0)/(x1-x0)*p10+(x1-x)/(x1-x0)*p00);

      // LOG(INFO) << " probability ===== : " << probability << std::endl;

      residualXi << 1.0 - probability;

      // free值越小, 表示占用的概率越大
      // residual[i] = scaling_factor_ * residual[i];

      // LOG(INFO) << " scaling_factor_ ===== : " << scaling_factor_ << std::endl;


      double dlx = (y-y0)/(y1-y0) * (p11-p01) + (y1-y)/(y1-y0) * (p10-p00);
      double dly = (x-x0)/(x1-x0) * (p11-p10) + (x1-x)/(x1-x0) * (p01-p00);
      Eigen::Matrix<double, 1, 2> jac_map;
      jac_map << dlx, dly;

      Eigen::Matrix<double, 2, 3> jac_pose;
      jac_pose.block<2, 2>(0, 0) = Eigen::Matrix<double, 2, 2>::Identity();
      float px = point[0];
      float py = point[1];
      jac_pose(0, 2) = px * rotation_matrix(0, 1) - py * rotation_matrix(0, 0); 
      jac_pose(1, 2) = px * rotation_matrix(1, 1) - py * rotation_matrix(1, 0);

      jacobianXi = jac_map * jac_pose;

      // LOG(INFO) << " y-y0 ===== : " << y-y0 << std::endl;
      // LOG(INFO) << " y1-y ===== : " << y1-y << std::endl;
      // LOG(INFO) << " x-x0 ===== : " << x-x0 << std::endl;
      // LOG(INFO) << " x1-x ===== : " << x1-x << std::endl;

      // LOG(INFO) << " jac_map ===== : " << jac_map << std::endl;
      // LOG(INFO) << " jac_pose ===== : " << jac_pose << std::endl;

      // LOG(INFO) << " jacobianXi ===== : " << jacobianXi.transpose() << std::endl;

      return true;
}

}  // namespace scan_matching
}  // namespace cartographer
